/*
 * CAN.c
 *
 * Created: 04.10.2012 11:35:04
 *  Author: bardbakk
 */ 
#include <avr/interrupt.h>

#include "definitions.h"
#include "MCP2515.h"
#include "CAN.h"
#include "USART.h"


void CAN_init()
{
	MCP2515_init();
	
	//Set loop-back mode
	MCP2515_bit_modify(MCP_CANCTRL, MODE_MASK, MODE_LOOPBACK);
	
	//Enable all receive interrupts
	MCP2515_bit_modify(MCP_CANINTE,0x3 ,MCP_RX_INT);
	
	//Receive all ids
	MCP2515_bit_modify(MCP_RXB0CTRL,0b01100000, 0xFF);
	
	//Set normal mode
	//MCP2515_bit_modify(MCP_CANCTRL, 0b11100000, 0x00);
	
}

void CAN_message_send(CAN_message_t msg)
{
	uint8_t status = MCP2515_read_status();
	int i;
	
	if (!BIT_CHECK(status, 2))
	{
		//Set Standard ID high and low, respectively
		MCP2515_write(MCP_TXB0CTRL+1,(uint8_t)(msg.id>>3));
		MCP2515_write(MCP_TXB0CTRL+2,(uint8_t)(msg.id<<5));
		
	
		//Set message length (max 8 byte)
		MCP2515_write(MCP_TXB0CTRL+5, (0b00001111 & msg.length));
	
		//Set data registers
		for(i = 0; i < msg.length; i++)
		{
			MCP2515_write(MCP_TXB0CTRL+6+i,msg.data[i]);
		}
		MCP2515_request_to_send(0);
			
	}
	else if(!BIT_CHECK(status, 4))
	{
		//Set Standard ID high and low, respectively
		MCP2515_write(MCP_TXB1CTRL+1,(uint8_t)(msg.id>>3));
		MCP2515_write(MCP_TXB1CTRL+2,(uint8_t)(msg.id<<5));
	
		//Set message length (max 8 byte)
		MCP2515_write(MCP_TXB1CTRL+5, (0b00001111 & msg.length));
	
		//Set data registers
		for(i = 0; i < msg.length; i++)
		{
			MCP2515_write(MCP_TXB1CTRL+6+i,msg.data[i]);
		}
		MCP2515_request_to_send(1);		
	}
	else if(!BIT_CHECK(status, 6))
	{
		//Set Standard ID high and low, respectively
		MCP2515_write(MCP_TXB2CTRL+1,(uint8_t)(msg.id>>3));
		MCP2515_write(MCP_TXB2CTRL+2,(uint8_t)(msg.id<<5));
	
		//Set message length (max 8 byte)
		MCP2515_write(MCP_TXB2CTRL+5, (0b00001111 & msg.length));
	
		//Set data registers
		for(i = 0; i < msg.length; i++)
		{
			MCP2515_write(MCP_TXB2CTRL+6+i, msg.data[i]);
		}
		MCP2515_request_to_send(2);	
	}
	else
	{
		
	}
}

char* CAN_error()
{
	return 0;
}

uint8_t CAN_transmit_complete()
{
	return 0;
}

CAN_message_t*  CAN_message_receive(uint8_t bufID)
{
	CAN_message_t* msg = malloc(sizeof(CAN_message_t));
	int i;
	switch(bufID)
	{
		case 0:
			//Read standard message ID
			msg->id = (uint8_t)( MCP2515_read(MCP_RXB0SIDH) << 3);
			msg->id +=  (uint8_t)(MCP2515_read(MCP_RXB0SIDH+1) >> 5);
			
			
			
			//Read message length
			msg->length =(0b1111) & (MCP2515_read(MCP_RXB0SIDH+4));
			
			//Read message data
			for(i = 0; i<msg->length; i++)
			{
				msg->data[i] = MCP2515_read(MCP_RXB0SIDH+5+i); 
			}
			break;
	}
	return msg;	
}	

ISR(INT2_vect, ISR_BLOCK)
{
	//Set LED for debugging
	BIT_CLEAR(PORTC, PC3);	
	
	CAN_message_t *msg;
	msg = CAN_message_receive(0);
	MCP2515_bit_modify(MCP_CANINTF, 0x1,0x1);
}